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ABSTRACT 



To control a lightweight manipulator, a validated dynamic model based on the 
Equivalent Rigid Link System (ERLS), is used to formulate a control algorithm. The 
required torque to drive the manipulator to a desired angle is calculated. Since the 
control system includes an electrohydraulic actuator, the required current is also 
calculated by the inverse dynamics of the hydraulic servo. A computer simulation is 
performed with various loading conditions, gain values, and desired angles. 



3 



TABLE OF CONTENTS 



I. INTRODUCTION 10 

A. MOTIVATION 10 

B. NATURE OF THE PROBLEM 10 

C. LITER.ATURE REVIEW 11 

D. RESEARCH OBJECTIVE 12 

II. MODEL FORMULATION FOR A SINGLE LINK FLEXIBLE 

MANIPULATOR 13 

A. PHYSICAL PLANT 13 

B. EQUIVALENT RIGID LINK SYSTEM FOR A SINGLE- 
LINK MANIPULATOR 13 

C. SHAPE FUNCTION DERIVATION 20 

D. HYDRAULIC ACTUATION 21 

III. CONTROL SCHEME 24 

A. REQUIRED TORQUE 24 

B. REQUIRED CURRENT 26 

IV. RESULT 29 

A. INPUTS AND MOTION OUTPUTS 29 

B. REQUIRED TORQUES 31 

C. CURRENT CALCULATIONS 32 

D. THE REDUCTIONS OF THE REQUIRED TORQUE 33 

E. THE REDUCTION OF VIBRATION 34 

F. THE ELECTROHYDRAULIC SATURATION 35 

V. CONCLUSIONS AND RECOMMENDATIONS 62 

A. CONCLUSIONS 62 

B. RECOMMENDATIONS 63 



4 



APPENDIX A: DERIVATION OF THE EQUATIONS OF MOTION 

FOR THE SINGLE - LINK FLEXIBLE 

MANIPULATOR 64 

APPENDIX B: SIMULATION PROGRAM 71 

LIST OF REFERENCES 85 

INITIAL DISTRIBUTION LIST 86 



5 



rn 



LIST OF TABLES 



1. ARM PAR.^METERS 14 

2. INITIAL CONDITIONS 29 

GAIN VALUES 30 

4. MAXIMUM OF TOTAL-ANGLE VELOCITY AND TOTAL-ANGLE 

ACCELEIUATION 32 

5. MAXIMUM REQUIRED TORQUE 33 

6. MAXIMUM TORQUE FOR INPUT FUNCTIONS 34 



6 



LIST OF FIGURES 



2.1 A Single- Link Flexible Manipulator System 15 

2.2 A Manipulator in Intermediate Position with 2.115kg Loading 15 

2.3 Details of Transverse Bridges and Tip Loading 16 

2.4 The Equivalent Rigid Link System (ERLS) 17 

2.5 An overall Plant 22 

3.1 A Schematic Diagram of the Feedback Control System 27 

4.1 Total Angle Response ( 0 kg ) 36 

4.2 Total Angle Response ( 2.115 kg ) 36 

4.3 Total Angle Response ( 4.233 kg ) 37 

4.4 Total-Angle Velocity (4.233kg) 38 

4.5 Total-Angle Acceleration(4. 233kg) 39 

4.6 Total Angle Responses for Various Desired Angles(2.1 15kg) 40 

4.7 Total-Angle Velocities for Various Desired Angles(2. 11 5kg) 41 

4.8 Total-Angle Accelerations for Various Desired Angles(2.1 15kg) 43 

4.9 Required Torque ( 0 kg ) 43 

4.10 Required Torque ( 2.115 kg ) 44 

4.11 Required Torque ( 4.233 kg ) 45 

4.12 Required Torques for Various Desired Angles(2.1 15kg) 46 

4.13 Required Current (0 kg) 47 

4.14 Required Current (2.115 kg) 48 

4.15 Required Current (4.233 kg ) 49 

4.16 A Planned Input of the Desired Angle(4.233 kg ) 50 

4.17 Total Angle Responses for Various Input Functions(4.233kg) 51 

4.18 Required Torques for Various Input Functions(4.233 kg ) 52 

4.19 Require Currents for Various Input Functions(4.233kg) 53 

4.20 Total Angle Responses for Various Damping Ratios(no load ) 54 

4.21 Large motion for Various Damping Ratios (no load ) 55 

4.22 Small Motion for Various Damping Ratios (no load ) 56 



7 



4.23 Required Torque for Various Damping Ratios (no load ) 57 

4.24 Required Currents for the Ideal and Saturated cases(4. 233kg) 58 

4.25 Required Torques for the Ideal and Saturated cases(4.233 kg ) 59 

4.26 Total Angle Responses for the Ideal and Saturated cases(4.233kg) 60 



8 



ACKNOWLEDGEMENT 



I wish to express my sincerest gratitude to my advisor, Professor Liang-Wey 
Chang, for his invaluable guidance and unselfish giving of his time in support of this 
research. 

.Appreciation is extended to the Korean Air Force Authority for the opportunity 
to study in Naval Postgraduate School. 

1 also want to thank my wife. Mi Sook and my daughter, Soo Yeun for their 
patience and encouragement in my study. 



9 



I. INTRODUCTION 



A. MOTIVATION 

The robotic manipulator plays an important role in a wide range of applications. 
Present and possible future applications focus on implementing automation in 
environments which are hostile to humankind. More demand is being placed on robots 
with higher speeds, smaller actuators, and higher payload capacity in applications. 

In the past, efforts were made to control manipulators using rigid body 
assumptions. However, the demands for lightweight and higher performance 
manipulators have led to the consideration of flexible- body models. A new controller 
is needed for the flexible manipulators. 

B. NATURE OF THE PROBLEM 

Control of most manipulators is based upon dynamic models assuming perfectly 
rigid links. Accordingly, accurate manipulator motion control is dependent upon the 
great stiffness of the structural members. This assumption does not always hold, 
particularly at high-speed and heavy-load operations where a linkage may undergo 
severe elastic deformation due to the inertia. It can no longer perform its intended 
function in actual operations. 

Lightweight flexible manipulators promise the advantages of increased speed of 
operation, ease of transportability, reduced material requirement, reduced power 
consumption, lowered mounting strength and rigidity requirement, and lower overall 
cost [Ref 1]. On the other hand, the oscillatory behavior of the flexible links makes 
the end-point motions difficult to be controled. The central problem lies in the control 
of the end-point motion by applying the proper torques at the actuating point. 
Flexibility effect on manipulators is a necessary consideration in the move toward 
designing lighter, faster and more accurate robot systems. A dynamics model of 
manipulators with flexibility is needed for design and control purposes. To benefit 
from the advantages of lightweight, flexible manipulators, it is also necessary to 
implement a control design capable of achieving end effector positioning accuracy and 
stable control. 
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C. LITERATURE REVIEW 



The earliest studies concerning the control of flexible manipulators began in the 
early to mid 1970's. There has been considerable research recently in the development 
of flexible manipulator control strategies. A brief survey of this research follows. 

Book [Ref 2] introduced a modeling approach utilizing 4x4 transformation for 
control purposes. His model was limited in the assumption that the mass of the 
manipulator is negligible compared to the mass of the load. The assumption is 
acceptable in space applications where the manipulator is ver>' light and operates at 
low speeds, but the assumption can not be applied to the industrial robotic 
manipulators in most cases. 

Sunada and Dubowsky [Ref 3] appUed 4 x 4 transformation matrix and the 
Lagrangian approach to model the flexible system with geometrically irregular links by 
finite element techniques. Component Mode Synthesis was used to reduce the number 
of generalized coordinates and improve the computational efficiency without losing 
much accuracy. Based on this scheme, they can obtain the flexibility and mass 
properties of the links through available finite element programs. But, this model 
ignored the effect of the small motion interaction on the large motion. The 
consideration of the coupling effect between the small motion and the large motion 
was not completed. 

Chang [Ref 4] introduced the Equivalent Rigid Link System(ERLS) dynamic 
model of flexible manipulator. Global motions were separated into large motions and 
small motions. The ERLS described the kinematics of the large motion. The small 
motion displacements were described relative to the ERLS. The finite element method 
was used to discretize deformations and Lagrangian formation was used to derive the 
equations of motion. Two sets of coupled, non-lmear ordinary differential equations - 
large motion equations and small motion equations resulted. The set of large motion 
equations were non-linear in both the large and small motion variables. The set of 
small motion equations were linear in the small motion variable but non-linear in the 
large motion variable. The model is complete being able to describe large motions, 
small motions and their coupling. The model presented the computational potential by 
using the Sequential Integration Method developed. 

Petroka [Ref 5] performed an experimental validation of the ERLS model. He 
designed a hydraulically actuated, single - link flexible arm. A cubic shape function 
was assumed to present the transverse displacement of the flexible manipulator. Tip 
position was determined from the motion picture studies. 
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Gannon [Ref. 6] served a continuation of th'e experimental validation of the 
ERLS model. A piezo-resistive accelerometer and strain gages were used for tip 
position and strain information. The natural mode shape functions of a beam was used 
to present the flexural motion of the single-link flexible arm in the Gannon's study. 
Results of the validation suggested the potential usefulness of the model in determining 
tip position. 

Canon and Schmitz [Ref 7] reported on experimental studies of the "End - Point 
Control of a Flexible One Link Robot". Their controller design was based upon a 
linearized and truncated model. Measurements available for control included rigid body 
angle and velocity, strain measurements and direct optical measurement of end-point 
position. Controller design was discussed from a variety of standpoints using both 
classical and modern control methods. The authors reported that the non 
-colocated(end -point position) feedback improves the speed of response at the cost of 
reducing the stability margins. 

Flexibility effect is a very important consideration in designing lighter, faster and 
more accurate robot systems. In this research, the ERLS dynamic model is used to 
design a control system since the model presents a complete consideration of the 
system motion and exhibits computational superiority. Nonlinear motion is 
compensated in the control algorithm. 

D. RESEARCH OBJECTIVE 

The objective of this research is to design a controller for single-link flexible 
manipulator. The validated dynamic model-the Equivalent Rigid Link System - is used 
to formulate the control algorithm. The control algorithm involves the computation 
of the required torque to drive the manipulator achieving a desired angle of the end of 
the flexible manipulator. Since the control system includes an electrohydraulic actuator, 
the required current is also calculated by inverse hydraulic dynamics. The ways to 
reduce the actuator effort and structural vibrations will be discussed. 
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II. MODEL FORMULATION FOR A SINGLE LINK FLEXIBLE 

MANIPULATOR 



A. PHYSICAL PLANT 

A flexible arm was designed by Petroka [Ref. 5] for the validation of the 
Equivalent Rigid Link System Model. The flexible arm shown in Figures 2.1 - 2.2 is a 
one meter long flexible structure which can be bend freely in the vertical plane but is 
stiff in the torsion and horizontal bending. The basic configuration of the arm includes 
two parallel, flexible steel, flat bars connected by the thin steel strips to transverse steel 
bridges. External loading is attached to the arm tip end transverse bridge by the 
securing of the load on the four welded studs shown in Figure 2.3. Table 1 shows the 
geometric and mass properties of the flexible arm. The flexible arm is driven by a 
elect ohydraulic system shown in Figure 2.1. The system consists of a York hydrauUc 
power unit, a Berd- Johnson 3 - axis Hyd-Ro- Wrist, a Moog 760*100 servovalve, and 
associated Moog servocontroller and high pressure filter assembly [Ref 5). 

B. EQUIVALENT RIGID LINK SYSTEM FOR A SINGLE-LINK 

MANIPULATOR 

The basic idea of ERLS [Ref 4] is to separate the motion of the flexible link 
system into the large rigid-body motion and the small-motion displacement due to the 
flexibility of the structure. The large motion is defined by the movement of ERLS for 
a single - link flexible manipulator. The small-motion displacements are described with 
respect the ERLS. A schematic diagram of the ERLS for a single - link manipulator is 
shown in Figure 2.4. 

The three generalized coordinates chosen are the large motion joint variable,0, 
and tw'o nodal displacements, V(0)and <p(0) ,which can be measured at the end of the 
link. The large motion joint variable ,0, is the angle between the ERLS and the 
horizontal axis of inertia coordinate. Homogenous transformations is applied for a 
deformed point on the single-link to obtain the absolute position of the point. The 
displacements for each point along the flexible arm are the functions of the location 
and time; therefore, it is necessary to discretize the deformations in terms of generalized 
coordinates in oder to apply Lagrangian's equation. The techniques of the Finite 
Element Method (FEM) are utilized for this discretization. The nodal displacements at 
the end of the single-link represent the small motion. 
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TABLE 1 

AR.M PARAMETERS 


Parameter 


Value 


Unit 


Arm Length (L) 


0.9985 


m 


Beam Thickness(t) 


0.003175 


m 


Beam Width(w) 


0.0762 


m 


Distance Between 


0.05715 


m 


Each Beam 






Arm Mass(m^) 


4.8565 


kg 


Modulus of Elasticity(E) 


2.0 Ell 


N/m^ 


Density 'unit Volume( p) 


7861.05 


kg/m^ 



After describing the kinematics relationships between the large and small 
motions, kinetics is introduced to complete the derivation of the equation motion. 

The Lagrangian dynamics approach is used for the derivation due to its 
straightforward and systematic nature which is helpful in the analysis of complex 
systems. The kinetic energy is the sum of the kinetic energies of each link, actuator, and 
any loading. The potential energy is contributed the elastic strain energy and from 
gravity. Generalized forces are included due to any applied forces and damping forces. 
After making considerable effort in mathematical manipulations, rearrangements, and 
simplifications, the Lagrangian equations yield two sets of non-linear, coupled, second 
-order, ordinary differential equations. One set describes the large motions and the 
other set describes the small motions. 

These equations are represented as follow; 

Mqqe+.Mqj^U =Fq+ TORQUE (2.1) 
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Figure 2.1 A Single-Link Flo.xible .Manipulator System. 



15 




Figure 2.2 A Manipulator in Intermediate Position with 2.115kg Loading. 
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Figure 2.3 



Details of Transverse Bridges and Tip Loading. 
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X. Y : INERTIA COORDINATE 

X. y ; LOCAL COORDINATE 

e : LARGE MOTION JOINT VARIABLE 

(p(0) ; ARM TIP SLOPE 

VfO) : ARM TIP DEFLECTION 

<l> ; TOTAL ANGLE 



Figure 2.4 The Equivalent Rigid Link System (ERLS). 



IS 



( 2 . 2 ) 




where 



Mqq =1x1 effective inertia matrix for large motions 
Mqj^ =1X2 coupled inertia matrix of the small motion effect 
on large motions 

=2X1 coupled inertia matrix of the large motion effect 
on the small motions 

=2x2 effective inertia matrix for small motions 
=2x2 stiffness matrix for small motions 
Fq =1x1 load vector for large motions 
=2x1 load vector for small motions 
0 = generalized coordinate of the large motions 



U =2x1 generalized coordinate of the small motions 

The vector of small motion is limited to the transverse displacement of the end of 
the link. Axial deformation and torsion are neglected in this research for single-link 
manipulators. We assume that the displacement of the end of the link is small. 



where V is a shorthand of V(0). 

We also assume that the slope of the arm tip is not measured for the simple 
feedback control system. The tip displacement V(0) is the only generalized coordinate 
chosen for small motions. Then all coefficient of the matrixs of non-linear, coupled, 
second-order ordinary differential equations is reduced to 1 x i matrix. A total angle 
of the end of the link can be represented as follows; 



Tan'ffV/L) ~ V/L 



( 2 . 3 ) 



o = e + V/L 



( 2 . 4 ) 



<i> = e + V/L 



( 2 . 5 ) 
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<D = 0 + V/L 



(2.6) 



A more detailed derivation of the equations of motion is included in Appendix A. 

C. SHAPE FUNCTION DERIVATION 

The natural- mode shape function of a beam is needed to present the flexural 
motion of the single-link flexible arm [Ref 4], The flexible manipulator arm is modeled 
as a continuous Euler-Bemoulli cantilever beam. The transverse displacement for the 
single link flexible arm is represented in the following forms; 

U(x,t) = a| (t) Xi (x} + a 2 (t) X 2 (x) (2-7) 

= aj {Aj' (cosPjX + cosh Pjx} + (sinPjX + sinhPjx}} 

+ a 2 (A 2 {cosp 2 X + cosh P 2 X} + (sin P 2 X + sinh P 2 X}} 

w'here 

PjL = 1.875104069 
P 2 L = 4.694091133 



' sin P;L + sinh P;L 

A; = 7T ^7 (2.8) 

^ cos PjL + cosh PjL 

Upon reduction, Garmon [Ref 6j yielded a 3 x 2 shape function matrix T, and 
the 2 X I nodal displacement vector, U. a 3 x 1 deformation vector, d , was 
expressed as follow; 



d = T u = 


’ 0 




' 0 


0 ' 


' V(0)' 




0 




0 


0 


. <P(0) . 




, V(x) ^ 




. M 


N . 





The coefTicents M and N are defined as; 

M = {Cj {Aj' { cosP jX+ coshp jX } + { sinPjX+ sinhPjx}) 

+ C2 { A2'(cosP 2X+ coshP2X} + (sinP2 x + sinhP2x}}} 
N = (Cj (Aj' { cosPjX+ coshPjX } + ( sinPjX+ sinhPjX)} 
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+ { A2'{cosP 2X+ coshP2x} + (sinP2 x + sinhP2x}}} 

where 

Cl = 2 P2/{4 Ai' P 2 - 4 A 2 ' Pi} 

C 2 =-2 Pi/{4 A|' P 2 * 4 A 2 ' Pi} 

C 3 =-2 A 2'/^4 Ai' P 2 - 4 A 2 ' Pi} 

C 4 " 2 ^l'/{4 h * ^ Pi} 

In this research, since the slope function of the arm tip is not measured, the 
shape function, T , is reduced to a 3 x 1 matrix. Now the 3 x 1 deformation vector 
can be rewritten as follows; 



d = T U = 


■ 0 


= 


' 0 ' 




0 




0 




V(x) _ 







New shape function matrix is now in a convenient form for computer coding. A 
more detailed derivation of the shape function was included in Gannon's work. 

D. HYDRAULIC ACTUATION 

The single-link flexible manipulator uses electrohydraulic actuation to drive the 
plant. The inclusion of the electrohydraulic power system involves the transformation 
of an input current to an output torque. Figure 2.5 illustrates the relationship of 
hydraulic dynamics to the overall system. 

The hydraulic dynamics is represented by servovalve dynamics and actuator 
dynamics. Moog, the manufacture of the servovalve, simplified the description of 
servovalve dynamics to a single equation [Ref 8). 



Q = I K V Py 



(2.9) 



where 

Q = flow delivered from servovalve 
I = input current 

K = valve sizing constant, contributes to hydraulic system damping 
Py = valve pressure drop,Pj-Pj^ 
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Figure 2.5 An overall Plant. 

Actuator dynamics consists of a form of the continuity equation and the torque 
output equation. .Actuator dynamics consists of a form of the continuity equation and 
the torque output equation [Ref 9]. 

• V Pt 

Q - Dm e + Pl + (2.10) 

Tj=ntPLDm - (211) 

Where 

Q = flow delivered to actuator 

Dj^ 6 = flow causing actuator rotation 

Pg = elTectivc bulk modulus of fluid 

Vj = total compressed volume including actuator lines and chambers 
\’t Pj^ ; (4 Pg ) = compressability flow 

= torque required to overcome inertia and move the load 
= torque efficiency 
Pf^ = load pressure drop 

Dj^ = motor displacement 

Ctm^L “ leaking flow in actuator 



T) 



The preceding hydraulic dynamic equations, and relationships are incorporated 
into the main computer program for solving the dynamic equation of motion. 
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III. CONTROL SCHEME 



A. REQUIRED TORQUE 

In feedback control system, desired angles are compared with the measurements 
of the actual system responses. The differance is used via the chosen control 
algorithm. The control algorithm calculates the required torque to derive the 
manipulator achieving a desired angle of the end of the arm. 

Recall the equations that can be written as two sets of equations - large motions 
and small motions equations - and the total angle equation of the end of the arm for 
derivation of the required torque equation. 



Mqq =1^1 effective inertia matrix for large motions 

=1x1 coupled inertia matrix of the small motion effect 
on large motions 

=1x1 coupled inertia matrix of the large motion effect 
on the small motions 

=1x1 effective inertia matrix for small motions 
=1x1 stiffness matrix for small motions 
Fq =1x1 load vector for large motions 
=1X1 load vector for small motions 
6 = generalized coordinate of the large motions 

V =1X1 generalized coordinate of the displacments 



^qq6+>lqnV =Fq+ TORQUE 



(3.1) 




(3.2) 



where 



0 = <D - V/L 



(3.3) 
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0 = <D - V/L 



(3.4) 



0 = 0) - V/L 



(3.5) 



Equations 3. 3-3. 5 can be substituted into Equations 3.1 and 3.2 and rearranged as 
follows; 

Mqq (i-V/L)+ Mqj, V - = TORQUE (3.6) 



(0-V7l)+ V + Kj, V -F^ = 0 (3.7) 

After eliminating V, a governing equation for is given as; 

(Mqq -D ) 0 + (D F^, -D - Fq ) V = TORQUE (3.8) 



where 



D = 



(^^on -(^00 /L» 

(Mnn-(Mnq/L» 



Let 



N = Mqq -D 



Equation 3.8 can be expressed in short as follows; 



(3.9) 



N ^ + FC (V,V,0,0) = TORQUE 



(3.10) 



where 



FC(V,V,0, 0 ) = (D F^ -D - Fq ) V 
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<D is measured and fed back to controller. . The required torque to drive the 
manipulator achieving a desired angle of the end of the arm is calculated via a 
controller. The inverse problem is applied to calculate the required torque. Figure 3-1 
shows a schematic diagram of the feedback control system. 

The equation of the required torque is represented as follows; 

T^ = N(Od+ Ky(4)^-<i>) + Kp(<D^-<D))+ FC (3.11) 

where 

Tj. = a required torque 

• • 

= a desired angular acceleration 
$ = a total angular acceleration 
<t>^ = a desired angular velocity 
<t> = a total angular velocity 
= a desired angle 
<I>= a total angle 
Ky = a velocity gain 
Kp = a position gain 

The torque of the Equation 3.10 is equals to the required torque of the Equation 
3.11. System errors of a second order control system can be represented as follows; 

( - ^ ) + Ky (4>d -<i>) + Kp -<D) = 0 (3.12) 

B. REQUIRED CURRENT 

Since the control system includes an electrohydraulic actuator, the inverse 
hydraulic dynamic involves the transformation of input torque to an output current. 
For the input current to the hydraulic dynamics, the inverse hydraulic dynamics can be 
applied to calculate the required current according to the required torque. A desired 
load pressure drop and desired flow should be calculated by the inverse hydraulic 
dynamic equations. 

Pu = ,(n, (3. [3) 
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Figure 3.1 A Schematic Diagram of the Feedback Control System. 
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Qr=Dme + C,mPLr+(V, Plr B, ) 



(3.14) 



I^=Qd/(KV^) (3.15) 

where 

Tj. = a required torque 

P^i- = a required load pressure drop 

Qj. = a required flow delivered to actuator 

Ij. = a required current 
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IV. RESULT 



A control algorithm for a single-link flexible manipulator is based upon the 
Equivalent Rigid Link System Model. The control algorithm calculates the required 
torque to control the end position of the manipulator. Comparisons of the required 
torque and required current are made in terms of the three parameters (i.e., loading 
condition, gain value, and desired angle). Additionally, the way to reduce the required 
currents and required torques is discussed. The effect of the current saturation to 
system dynamics behavior is also presented. 

A. INPUTS AND MOTION OUTPUTS 

The loading conditions are no load, a 2.115 kg ,and a 4.233kg load. The 
external loading is attached to the arm tip end transverse bridge by securing the load 
on the four welded studs. The initial condition for all runs is the horizontal position of 
the flexible arm. The initial tip deformations as determined by the Euler-BerouUi 
cantilever beam theory are converted to an angle. These angles in Table 2 are the 
initial conditions input into the simulation program. 



TABLE 2 

INITIAL CONDITIONS 


Load ( Kg ) 


Angle (radian) 


0.0 


-0.06111 


2.115 


-0.14628 


4.233 


-0.23155 



Only point to point control in open space is considered. An input variable to the 
closed-loop system is the desired angle of the end-point. The desired angular 
acceleration and the desired angular velocity are zero in this research. To compare the 
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system responses, two step inputs, which are two desired angles of the end-point,(i.e., 1 
radian and 1.5 radian), are applied respectively. 

The outputs of the system responses for the step input of the desired angle are 
total-angle accelerations, total-angle velocities and total angles of the end-point, which 
are also fed back to the controller. First, the desired angle of 1 radian is input as the 
step function. Figures 4. 1-4.3 show the movements of the end-point for all loading 
conditions. Each figure has three curves according to selected gain values for all 
loading conditions. In this research, a velocity gain ( Ky) and a position gain (Kp) are 
selected for critically damped systems. In other words, the movements of the total 
angle of the end-point will not exceed the desired angle of the end-point. The position 
gain can be expressed in terms of the velocity gain for the critically damped systems as 
fellows; 

Kp - , 4 

Table 3 shows the three cases of gain values for this research: 



TABLE 3 
GAIN VALUES 


Case 


Velocity Gain 


Position Gain 


1 


2 


1 


2 


4 


4 


3 


8 


16 



For the evaluation of the system performances for the step input, a settling time 
corresponding to a 2 % tolerance is measured in terms of time constant( T = 1 / 
Settling time( t. = 4 T ) are 4 sec, 2 sec and 1 sec according to the position 
gains 1, 4 and 16. All results of the total angles of the end-point meet error criteria of 
2 % according to the gain values. The figures of the total angle of the end-point with 
larger gain values show faster movement. Another observation is that the total angle 
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of the end-point with the same gain value have the. same movement in terms of the 
time constant for the different loading conditions. These results indicate that the 
nonlinear plant is controlled to perform a linear behavior. Errors of the system can be 
represented as the second-order control system as follows; 

( Oj - <^ ) + Ky (0>j -4)) + Kp (d>^ -0>) = 0 

Figures 4.4-4.S show the total-angle velocity and total-angle acceleration 
approaching the desired angle of 1 radian for the 4.233 kg loading condition. All 
initial velocities start from zero and being increasing to achieve the desired angle with 
fast movement as gain values are increasing during transients and converge to zero 
velocity as the total angle of the end-point approachs to the desired angle. 

Figure 4.6 shows the comparison of the total angle response for the step inputs 
of 1 radian and 1.5 radian with the position gain of 4, the velocity gain of 4 and 2.115 
kg loading condition. The total angle movement of the 1.5 radian is faster than the 
movement of the desired angle of 1 radian. But both of the total angle response have 
same settling time since they have the same position (4) and velocity gain values(4). 
Figures 4. 7-4. 8 show the comparisons of the total-angle velocity and total-angle 
acceleration between the 1 radian and 1.5 radian of the desired angle with same 
position gain for the 2.115kg loading condition. The figures of the total-angle velocity 
and total-angle acceleration indicate that total-angle velocities and accelerations are 
increasing as desired angle is increasing with same gain value during the transients. 
The maximum of total-angle velocities and total-angle accelerations for the desired 
angles are shown in Table 4. 



B. REQUIRED TORQUES 

In the feedback control system, the desired angle of the end-point is compared 
with the measurements of the actual system responses. The differences are used via the 
chosen control algorithm to calculate the required torque. In other words, the control 
algorithm calculates the required torques to drive the manipulator to achieve the 
desired angle of the end-point according to the chosen gain values. The equation of 
the required torque was represented as follows; 

T, = N ( 4 - Ky (4>d -0>) + Kp (4>d -0))+ FC 
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TABLE 4 

MAXIMUM OF TOTAL-ANGLE VELOCITY AND TOTAL-ANGLE 

ACCELERATION 



1 

j Desired 

1 Angle(rad) 

t 


Maximum Angular 
Velocity(rad,'sec) 


.Maximum ang^ular^ 
Acceleration! rad sec^) 


1 1.0 


0.974 


37.61 


1.5 


1.29 


37.78 



Figures 4.9-4.11 show the required torques to drive the manipulator in order to 
achieve the desired angle of 1 radian for all loading conditions. The desired angle of 1 
radian is input as a step function. All figures have three curves according to the three 
gains in Table 1. Table 5 shows the maximum of the required torques according to the 
gain values and the loading condition. The figures for the required torques and Table 
5 show expected results; the required torques during the transient period are increasing 
to deliver the load and to overcome the inertia moment as gain values are increasing, 
and rapidly settle to the steady-state value. These required torques are related to the 
movements of total angle of the end-point. For the faster movements of the total 
angles, more required torque should be applied to the physical plant. Because of no 
steady-state error, the required torques at the steady-state ( the total angle reaches to 
the desired angle ) have same values for the same desired angle even though the gains 
are different. 

Figure 4.12 shows the required torques for desired angles(i.e.,l radian and 1.5 
radian). The comparison for the desired angles indicates that the maximum of the 
required torque depend upon the movements of the total angle is increasing as the 
desired angle is increasing. 

C. CURRENT CALCULATIONS 

The control algorithm includes an electrohydraulic actuator. The input current 
to the manipulator can be calculated from the inverse electrohydraulic dynamics 
according to the required torque. Figures 4.13-4.15 represent the graphical results of 
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TABLE 5 

MAXIMUM REQUIRED TORQUE 


Load(kg) 


Postion Gain 


Torque(N-m) 


No Load 


1 


82.559 


4 


84.041 


16 


89.969 


2.115 


1 


148.11 


4 


155.64 


16 


185.76 


4.233 


1 


125.73 


4 


139.68 


16 


183.44 



the required currents for all loading conditions. Each figure has the three curves 
according to three gain values. Figures indicate that the required currents have 
fluctuations based upon required torques during the transients, and settle to the 
steady- state values. 

D. THE REDUCTIONS OF THE REQUIRED TORQUE 

The magnitudes of the required torques and required currents are related to the 
selection of size of the electrohydraulic actuator. The selections of the electrohydraulic 
actuator which depend upon the required torques and required currents, are also 
related to the weight,and cost. To reduce the required torques and required currents is 
very important in terms of the selection of the size of electrohydraulic actuator. In this 
research, a planned desired angle trajectory is used as the input replacing the step 
input. 
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The planned input is the combination of ramp and step function applied to the 
4.233kg load with the position gain of 16. The initial point of the ramp function 
coincides with the initial angle of the flexible arm. The slope of the ramp function is 
selected as 2. Figure 4.16 shows the planned input of the desired angle as the 
combination function. Figures 4.17-4.19 show comparisons of the results between the 
step input and planned input. The difference of the maximum required torque for 
input functions is tabulated in Table 6. 



TABLE 6 

MAXIMUM TORQUE FOR INPUT FUNCTIONS 


Input Function 


Max. Torque(N-m) 


Step input 


183.41 


Planned Input 


106.58 



Table 6 indicates that the plan of the input function for the desired angle could 
results in the reduction of the maximum required torque and maximum required 
current. The total angle responses might be slower. We can expect different reductions 
of the required torques or required currents for same loading conditions and gain value 
when we change the input function of the desired angle. 

E. THE REDUCTION OF VIBRATION 

The system damping is considered to reduce the structural vibration. A damping 
ratio is applied to the small motion equation as follows; 

M 9+M V+2CWV-I-K V=F 
^ *nq ^ ‘ ‘nn v-r.:«,^n n n 

The no load fundamental frequency is predicted to be 2 HZ. A damping ratiof Q of 
0.5 is applied to no load condition with the position gain of 2. Figures 4.20-4.23 show 
graphical results of the comparison of the no damping case with the damping case that 
has a 0.5 damping ratio. These result show that the movement of the end-point of the 
damping case is faster than the movement of the no damping. The system damping 
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yields the noticeable reduction of the vibration of small motion. The required torques 
of the system damping case show less fluctuations. 

F. THE ELECTROHYDRAULIC SATURATION 

The previous results for the required torques and required currents are for an 
ideal condition. When the electro hydraulic actuator is saturated at 15 ntilliamp 
current, all current exceeded 15 milliamp will be limited at 15 milliamp current. 
Figures 4.24-4.26 show the effect of the saturation for the 4.233 kg loading condition 
with the position gain of 16. The total angle responses with the saturated current show 
slightly slower movements compared to the ideal movement during transients. The 
required torques are also reduced due to the saturated current during the initial 
transients. 
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Figure 4.1 Total Angle Response ( 0 kg ). 
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Figure 4.2 Total Angle Response ( 2.115 kg ). 
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Figure 4.3 Total Angle Response ( 4.233 kg ). 
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Figure 4.4 Total-Angle Velocity (4.233kg). 
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Figure 4.5 Total-Angle Acceleration(4. 233kg). 
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Figure 4.6 Total Angle Response for Various Desired .Vngles(2.1 15kg). 
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Figure 4.7 Total-Angle Velocities for Various Desired .Angles(2.1 15kg). 
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Figure 4.8 Total-Angle Accelerations for Various Desired Angles(2.1 15kg). 
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Figure 4.9 Required Torque ( 0 kg ). 
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Figure 4,10 Required Torque ( 2.1 15 kg ). 
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Figure 4. 1 1 Required Torque ( 4.233 kg ). 
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Figure 4.12 Required Torques for Various Desired Angles(2.1 15kg). 
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Figure 4.13 Required Current (0 kg). 
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Figure 4.14 Required Current (2.115 kg). 
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Figure 4.15 Required Current (4.233 kg ). 
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Figure 4.16 A Planned Input of the Desired Angle(4.233 kg ). 
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Figure 4.17 Total Angle Responses for Various Input Functions(4. 233kg). 
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Figure 4. IS Required Torque for Various Input Functions(4.233 kg ). 
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Figure 4.19 Require Current for Various Input Functions(4. 233kg). 
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Figure 4.20 Total Angle Response for Various Damping Ratios(no load ). 
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Figure 4.21 Large motion for Various Damping Ratios (no load ). 
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Figure 4.22 Small Motion for Various Damping Ratios {no load ). 
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Figure 4.23 Required Torque for Various Damping Ratios (no load ). 
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Figure 4.24 Required Currents for the Ideal and Saturated cases(4. 233kg). 
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Figure 4.25 Required Torques for the Ideal and Saturated cases(4.233 kg ). 
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Figure 4.26 Total Angle Responses for the Ideal and Saturated cases(4. 233kg). 
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V. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

The purpose of this research is to design a controller for a single-link flexible 
manipulator using the Equivalent Rigid Link System. The control algorithm is 
formulated calculating the required torque to control the desired angle of the end- 
position. Comparisons of the system responses and required torques were made in the 
different conditions. 

The results indicate that system responses and required torques are related to the 
selected gain values for all loading conditions. The movements of the end-point with 
higher gain values are faster to achieve the desired position. For different desired 
positions with the same gain, the end-point movements have the same pattern. These 
results indicate that non-linear motion is compensated in the control algorithm. 

The required torques are increasing to deliver the load and overcome the inertial 
forces during transient times as gain values are increasing. The system rapidly settles to 
the steady state. Because of no steady state error, the required torques at the steady 
state have same values for the same desired positions even though the gains are 
different. 

The maximum of the required torques and required currents are related to the 
capacity of the electrohydraulic actuator. The selection of size of electrohydraulic 
actuator affects the weight and cost of the control system. Since more demand is being 
placed on robot control with higher speeds and smaller actuators, it is important to 
reduce the maximum required torques and required currents. The apphcation of the 
planned input of the desired angle as a combination of ramp and step functions lead a 
noticeable reduction of the maximum required torques and required currents. However, 
slower movements of the end-point are yielded during the transient period. The 
maximum required torques and required currents can be adjusted by the slope of the 
ramp function. 

The results also indicate that the system damping provides a noticeable reduction 
of the structural vibration. 

In addition, when the electrohydraulic actuator is saturated, the movement of the 
end-point is slower than the ideal case without saturation. The simulation serves an 
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useful method to run the physical plant within the operating limit of the selected 
electro hydraulic actuator. 

B. RECOMMENDATIONS 

The long-term goal of this research is to design a controller being capable of 
achieving end effector positioning accuracy and stable movements. Simulation studies 
need to be continued for flexible multi- link system design using the ERLS model. In 
addition, the effects of the robustness and spillover problems need to be studied. 
Extention of the controller design and implementation to the multi-link of the ERLS 
model is eventually needed if the advantages of flexible manipulator is to be realized in 
practical industrial application. 
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APPENDIX A 

DERIVATION OF THE EQUATIONS OF MOTION FOR THE SINGLE - 

LINK FLEXIBLE MANIPULATOR 

The motion of the flexible link manipulator has been separated into large 
motions and small motions by the ERLS. The generalized coordinates are chosen to 
describe the large motions by the joint variable of ERLS and to describe the small 
motions by nodal displacements of link. The two generalized coordinates chosen are 
the rigid body rotation, 6, and the nodal displacement, U{0) which can be measured at 
the end of the link. The following are the two sets Lagrange equations used to develop 
the equations of motion; 

d'dt{ aKE/c0} - ^KE/^0 + ^PE/^0 = F 

d/dt{ ^KE/^U } - ^KE/aU + ^PE/5U = 0 



where 

KE - kinetic energy 

PE - potential energy 

0 - large motion joint variable measured between the ERLS link and 
the absolute x axis. 

U - small motion displacement and slope 

F - generalized force for large motion 

The total kinetic energy is due to kinetic energy of the link motion (KE)^ and 
that of the actuators (KE)^; 

KE = ( KE )l + ( KE 

The actuator are much more rigid than the link, the actuator is treated as a rigid 
body and all kinetic energy terms are calculated separatly. Generalized forces include 
any applied force and damping forces. The expressions utilized for the determination of 
the kinetic energy of the system are as follows; 

KE( arm ) = 1/2 J ^ R^ (R) dv 

arm 

volume 
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KE( load) = 1/2 Trace J 



load 

volume 



KE( rotor) = 1/2 Trace J R/ {Rj.} dv 



rotor 

volume 



The absolute position vector of the arm is determined from the following 
transformation; 

R = W (r + D ) 

W = the 3 X 3 transformation matrix of function of theta.0, only, 
r = the 3 x 1 local position vector of the arm measured from the 
coordinate system whose origin is the end of the ERLS link, 
d = the 3 x 1 deformation vector that only includes the transverse 
displacement, derivation of this vector is discussed in chapter 2 . 

^ = the mass density of the steel arm. 

The absolute position vector of load is determined from the following 
transformation; 



Rl = W Dl Tl 

= the 3 X 3 transformation matrix due to the local deformation 
of the arm tip. 

rj^ = the 3 X I local position vector for the load 
= mass density of the steel load 

The absolute position vector of the hydraulic actuator is given by the following 
transformation; 

Rr =^r '■r 

= the 3 X 3 transformation matrix due to the large motion rotation 
of the rotor 

rj^ = the 3 X 1 local position vector for the rotor 

= the mass density of the actuator rotor, aluminum 
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Potential energy of the flexible arm comes -from strain energy (PE)_ of the 
structure due to deformation and the gravitational energy (PE)g of the system. The 
strain energy is produced by bending, tortion and extention. Along modeling, the 
lateral shear deflections are neglected so that the system energy of bending is due to 
flexure only. The expressions used for the determination of potential energy are as 
followes; 

PE^ = 1/2 J E 1,^ (v")2 dx 

link 

length 



PE = - 1 ft r^ g dv 

link 

volume 



PEgL = 

load 

volume 



where 

E 1^2 = the flexural rigidity in the z direction, perpendicular to 
the plane of motion 

V = second derivative of the transverse displacement V with respect 
to the local x coordinate direction expressed interms of V(0) 
g = gravitational acceleration vector 

The following definitions for the inertia terms are utilized to simplify the 
computations and the resultant expressions in the equations motion; 

1^ = the 3 X 3 inertia matrix of the load 

load 

volume 

= the 3 x 3 inertia matrix of the rotor 

= J '■R '■R^ 

rotor 

volume 
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I111(W0^ , Wg ) = J ^ W0^ Wg r dv 

link 

volume 



1 1 12(Wg^ , W ) = J ^ Wg^ W T r dv 

link 

volume 



I121(W^ , Wg ) = j ^ Wg^ W dv 

link 

volume 



I122(W^ , W ) = J n W T dv 

link 

volume 



I122(W^ . Wr ) = J H Wj^ T dv 

link 

volume 



I122(Wg^ , W ) = J ^ Wg^ W T dv 

link 

volume 



I122{Wg^ . Wg ) = J H Wg^ Wg T dv 

link 

volume 



^>7 J 
^xx = hi 

where 

*F = the 3 X 2 link shape matrix 

Wg = derivative of the 3 x 3 matrix W with respect to the joint variable 6 
• # 

Wj^ = result of the simplification of the second time derivative of 

the transformation matrix W and is termed the residual acceleration. 



The following definitions are utilized to simplify the computations and the 
expressions used for computer coding of the equations of motion; 
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Kj, = Jr^c r dx 

link. 

length 

Hii = J n dv 

link 

volume 



H21 = f Ml T^dv 

link 

volume 



H4I = J ‘‘L^ 

link 

volume 



r = the second derivative of the shape function matrix 
C = the 3 X 3 flexural rigidity matrix including only E 1^^ 

The actual derivation of the equations of motion requires detailed, tedious 
substitution of the expressions for the kinetic energy and potential energy into the 
Lagrange equations, and the Lagrangian formulation yields two sets of equations, One 
set describes the large motions and the other set describes the small motions. These 
two sets of equations are non- linear, coupled, second - order, ordinary differential 
equations represented as follows; 



where 



-^qq 


e + >lqn 




e +^nn 


^qq 


= 1X1 


'^qn 


= 1X2 




motions 




= 2X1 




motions 


^nn 


= 2x2 



= 2x2 stiffness matrix for small motions 
= 1X1 load vector for large motions 
= 2 x 1 load vector for small motions 
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6 = generalized coordinate of the large motions 

U = 2 X 1 generalized coordinate of the deformations representing 
the small motions-the displacement and slope. 

The coefficients for the terms are defined as follows; 

Mqq - IHKWj' , Wj)+ U' II22(Wj' , VVj) U 

+ Trace ( Wg 1^ D^' Wg' + Ar Ir ) 

>Iq„ = I112(Wg'.Wg) + ((Mi_ L + MJ,( L + 

Fq = 2 I122 (Wq^ , W) U + Hjj Wq^ + 

U' H,| Wg' g - Trace ( Wg Dl D.^' W, 

+ 2 Wq Dl II W 

+ AReiRARR')+H||DL‘Wgg + T 

M„q - (Trace{WgDi_lLDL„'w' ), 

Trace( Wq Dl II Dl12^ ^^21( W\+1121(W^ , Wq ) 

M„„ - 1122 (W‘ , W ) + Ml ( v( 0) ) + ( + lyy ) X ( $(0) ) 

K„ = k" + il22 ( W' , Wr ) 

F„ = Hj, W' g - ( Trace ( Wr It Dl„ W' ), + 2 W D,_ II Dl, W< ), 
Trace( Wr Dl II Dl,/ W' + 2 W Dl II Dl,/ W' )> 

+ H4,DL,,‘w'g. Hq, DL,/w'g 

where 

L = the arm length 
Ml = the mass of the load 

Mj. = the first moment of the load with respect to the local y axis. 

T = the externally applied torque 

Arr = a result of simplification of the second time derivative of the 

transformation matrix Ar and is termed D |2 residual acceleration. 

Ar6 = a derivative of Ar with respect to the joint variable theta. 

Dlii,Dli 2 ~ ^he derivatives of the arm tip deformation transformation matrix 
differentiated with respect to nodal deflection displacement and the nodal slope 
displacement respectively. 
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These expressions are coded and solved for in the computer program listed in 
Appendix B. 
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APPENDIX B 
SIMULATION PROGRAM 



TITLE THESI9V(2.11kg) 

CONST GKV=2.0, GKP=1.0 ,DEA=1.0 

* THESIS COPY 

* 



SIMULATION FOR DESIGN A CONTROL ALGORITHM 



THIS PROGRAM IS TO CALCULATE THE REQUIRED TORQUE AND REQUIRED 
CURRENT TO CONTROL THE END POSITION OF THE SINGLE- LINK ARM. 

THE ARM PARAMETERS ARE INPUTTED AND THE HYDRAULIC ACTUATION 
DYNAMICS ARE INCLUDED IN THE SIMULATION. 

THE INPUTS OF THE SYSTEM ARE THE DESIRED ANGLE OF THE END OF ARM. 
THE OUTPUTS OF THE SYSTEM ARE THE TOTAL ANGLE, TOTAL AI^JGULAR 
VELOCITY, ANGULAR ACCELERATION WHICH ARE FED BACK TO THE 
CONTROLLER. THE CODING CONSISTS OF A MAIN 
PROGRAM AND SUBROUTINES AND ARE DESCRIBED BELOW. 
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THE FOLLOWING PARAMETERS ARE DEFINED: 

1. A-EFFECTIVE CROSS-SECTIONAL AREA OF FLEXIBLE ARM 

2. ARRDD-3X3 SECOND TIME DERIVATIVE OF ROTOR RESIDUAL ACCELERATION 

MATRIX 

3. ARTH-3X3 ROTOR TRANSFORMATION MATRIX DIFFERENTIATED WITH RESPECT 
TO THETA 

4. BE-EFFECTIVE BULK MODULUS OF FLUID 

5. BIGF-2X1 RIGHT-HAND SIDE VECTOR FOR LARGE AND SMALL MOTION 

ACCELERATIONS 

6. BIGM-2K2 MATRIX OF LARGE AND SMALL MOTION ACCELERATION 

COEFFICIENTS 

7. CTM-TOTAL LEAKAGE COEFFICIENT OF THE ACTUATOR 

8. DEFM- DISPLACEMENT DEFORMATION VARIABLE 

9. DEFMD-TIME DERIVATIVE OF DISPLACEMENT DEFORMATION VARIABLE 

10 . DIFF , QERR , QERRl , FACTOR-DUMMY VARIABLES 

11. DL1-3X3 DEFORMATION MATRIX 

12. DL11-3X3 DEFORMATION MATRIX DIFFERENTIATED WITH RESPECT TO THE 
DISPLACEMENT DEFORMATION VARIABLE 

13. DL1D-3X3 FIRST TIME DERIVATIVE OF DEFORMATION MATRIX 

14. DM-ACTUAT0R DISPLACEMENT 

15. E-MODULUS OF ELASTICITY OF STEEL 

16. FN- RIGHT-HAND SIDE FOR SMALL MOTION ACCELERATIONS 

17. FQ-RIGHT-HAND SIDE FOR LARGE MOTION ACCELERATIONS 

18. G-3X1 GRAVITATIONAL ACCELERATION VECTOR 

19. H11-1X3 LINK FIRST MOMENT OF INERTIA VECTOR 

20. H21-1X3 LINK SHAPE MATRIX FIRST MOMENT OF INERTIAVECTOR 

21. H41-1X3 LOAD FIRST MOMENT OF INERTIA VECTOR 

22. KCE-TOTAL FLOW PRESSURE COEFFICIENT 

23. PL-LOAD HYDRAULIC PRESSURE DROP 

24. PS-HYDRAULIC SUPPLY PRESSURE 

25. QL-FLOW DELIVERED FROM THE SERVOVALVE 

26.SOL-2X1 VECTOR OF LARGE AND SMALL MOTION ACCELERATIONS 

27. TE-TORQUE EFFICIENCY 

28. TH-LARGE MOTION POSITION VARIABLE 

29. THD-TIME DERIVATIVE OF LARGE MOTION VARIABLE 

30. TORQUE-APPLIED TORQUE BY ACTUATOR 

31. U-2X1 ARM TIP DEFORMATION OF DISPLACEMENT 

32. UD-ARM TIP DEFORMATION DIFFERENTIATED WITH RESPECT TO TIME 

33. VT-TOTAL COMPRESSED VOLUME INCLUDING ACTUATOR LINES AND CHAMBERS 

34. W-3X3 LINK TRANSFORMATION MATRIX 

35. WD-3X3 FIRST TIME DERIVATIVE OF LINK TRANSFORMATION MATRIX 

36. WRDD-3X3 SECOND TIME DERIVATIVE OF LINK RESIDUAL ACCELERATION 
MATRIX 
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37. WTH-3X3 TRANSFORMATION MATRIX DIFFERENTIATED WITH RESPECT TO 
THETA 

38. XIFRAC-VARIABLE FRACTIONAL AMOUNT OF INPUT CURRENT TO SERVO- 
VALVE 

3 9. XI INP- CURRENT INPUT EQUAL TO INITIAL AND FRACTIONAL AMOUNTS 

40. XIL-3X3 INERTIA MATRIX OF THE LOAD 

41. XI0-INITIAL INPUT CURRENT TO SERVOVALVE 

42. XIR-3X3 ROTOR INERTIA MATRIX 

43. XK11- PARTIAL LINK STIFFNESS 

44. XKN- LINK STIFFNESS 

4 5. XKV- SERVO VALVE SIZING CONSTANT 

46. XLL-LENGTH OF FLEXIBLE ARM 

47. XML-MASS OF LOAD 

48. XMNN-C0EFFICIENT OF SMALL MOTION ACCELERATIONS IN THE 
SMALL MOTION DYNAMIC EQUATIONS 

49. XMI^JQ- COEFFICIENT OF LARGE MOTION ACCELERATIONS IN THE 
SMALL MOTION DYNAMIC EQUATIONS 

50. XMQN- COEFFICIENT OF SMLL MOTION ACCELERATIONS IN THE 
LARGE MOTION DYNAMICS EQUATION 

51. XMQQ-COEFFICIENT OF LARGE MOTION ACCELERATION IN THE LARGE MOTION 
DYNAMICS EQUATION 

52. XMQQP- DUMMY MATRIX FOR USE IN FORMULATING THE EQUATIONS OF 
MOTION 

53. XMR-MASS OF ACTUATOR ROTOR 

54. XMU-MASS DENSITY OF STEEL FLEXIBLE ARM 

55. XMX-FIRST MOMENT OF LOAD WITH RESPECT TO THE LOCAL COORDINATE 
Y AXIS 

56. ZI -AREA MOMENT OF INERTIA OF FLEXIBLE ARM 

57. TAg- TOTAL total angle 

58. TAG1- TOTAL ANGULAR VELOCITY 

59. T.AG2- TOTAL ANGULAR ACCELERATION 

60. RETO- REQUIRED TORQUE 

61. REIC -REQUIRED CURRENT 



INITIAL VALUES OF PARAMETERS ARE INPUTTED VIA XINIT SUBROUTINE 



DIMENSION U( 1),XM' 
#XIR(3,3) .XMQN(1^ ),UD 



INITIAL 

"k 
k 
k 

D 
D 
D 
D 
D 
D 
D 
D 
D 
D 

D #DEQ(i);DI(l),DEPL(l) 

FIXED I 

NOSORT 

D COMMON/ FCDATA/C1,C2, 



l),^QQPa KDL1(3 3),WTH(3 3),ARTH(3,3), 
1 ),Hll(l,3j,G(3,l) ,H21(1,3) , 



#WRDD(3,3) ,DL1D(3,3) ,WD(3,3) ,ARRDD(3 , 3) ,H41 (1 , 3) ,XK11(1 ), 

ttXMNQ(l ) ,W{3,3) ,XMNN(1 ),XKN(1 ),FN(1 ),BIGM(2,2), 

#BIGf(2 l),XIL(3,3),DLll(3,3),pEFMD(l).SOL(2hTHDaj, 
#A(1),E(1) ,ZI(1) , FQ(l),GPOS(3),XITH(l), 

#XMU(1) ,XLL(1) ,XML(1) ,XMR(1),XMX(1) ,TH(1) , TORQUE (1 ) ,DEFM(l) , 
#Ps7n,CTM(n,VT(p BE(l),pM(l),XKV{lKTE(l) 
#QL(iKpL(1),DIFF(1KXIINP(1) ,QERRl(l),QERR(l),FACTOR(l), 
#rEIC(l) , rETO(l) ,FTT(1) ,CTB(l),FCO(l), 



A1P,A2P,BETA1,BETA2 



C1=0. 515462194 
C2=-0. 205906794 
A1P=1. 362220557 
A2P=0. 981867539 
BETA1=1, 877920950 
BETA2=4. 701142847 

CALL XINIT (TH,THD,DEFM,DEFMD ,VO,POSO,A,XML,XMU, . . . 
XLL,XMR,E,ZI,PS ,CTM,VT,BE,DM,XKV,TE,QL, . . . 

^ PL.PLIC, REIC) 

DERIVATIVE 

k 

* COEFFICIENTS FOR BOTH LARGE AND SMALL MOTION ACCELERATIONS 

* AND THE RIGHT-HAND SIDES ARE COMPUTED IN THE FOLLOWING 
SUBROUTINES. ALSO, THE HYDRAULIC DYNAMICS ARE INCLUDED 

* IN THE MAIN PROGRAM. 
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^ if 



HYDRAULIC DYNAMICS 



* 

NOSORT 



XIINP(1)=DEIC(1) 

IF(PL(1) .GT.PS(l) ) GO TO 2 
GO TO 3 

2 PL(1)=PS(1) 

3 QERR1(1)=(XIINP(1)*XKV(1)*DSQRT(PS(1)-PL(1)))-(DM(1)*THD(1)) 

QERR(1)=QERR1(1)/CTM(1) 

DIFF(1)=QERR(1)-PL(1) 

FACTORU)=VT(1)/(4.0DO*BE(1)*CTM(1)) 

DIFF1(1)=DIFF(1) 

SORT 

PL1=INTGRL (PLIC , DIFFl , 1 ) 

NOSORT 

PL ( 1 )=PL1 ( 1 ) /FACTOR( 1 ) 

^ TORQUE(l)=TE(l)*PL(l)^DM(l) 

* MATRIX AND VECTOR FORMULATION SUBROUTINE 

CALL FORM (W , WTH , WD , DL 1 , DLID , XIL , XIR , ARTH , WRDD , ARRDD , U , UD 

XMQQP,G,H11,H21,DL11 ,H41 ,XK11 ,A,XMU,XML,XLL,TH,THD, . . . 

DEFM,DEFMD ,E , ZI , XMR, XMX ) 

X 

* COEFFICIENT OF LARGE MOTION ACCELERATION IN LARGE MOTION DYNAMICS 

* EQUATION SUBROUTINE 

^ CALL XLMMQQ(XMQQ,U,XMQQP,DL1,WTH,ARTH,XIL,XIR,A,XMU,TH,DEFM ) 

* COEFFICIENTS OF SMALL MOTION ACCELERATIONS IN LARGE MOTION DYNAMICS 

* EQUATION SUBROUTINE 

^ CALL XLMMQN(XMQN,A,XMU,XML,XLL,XMX,DEFM) 

* RIGHT-HAND SIDE FOR LARGE MOTION DYNAMICS EQUATION SUBROUTINE 

CALL XLMFQ(FQ,U,XMQQP,DL1, WTH, ARTH, XIL, XIR, UD,H11,G,H21, WRDD, . . . 

DL 1 D , WD , ARRDD , H4 1 , TH , THD , DE FM , DEFMD , A , XMU , XML , XLL 

^ TORQUE, FTT) 

* LINK STIFFNESS MATRIX SUBROUTINE 

CALL SMKN(XKN,XK11,XMQQP,A,XMU,THD) 

•k 

* COEFFICIENTS OF LARGE MOTION ACCELERATION IN SMALL MOTION 

* DYNAMICS EQUATIONS SUBROUTINE 

CALL SMMNQ(XMNQ,DL1,WTH,XIL,DL11 ,W,TH,DEFM, A,XMU, . . . 

XLL) 



RIGHT-HAND SIDE OF SMALL MOTION DYNAMICS EQUATIONS SUBROUTINE 

CALL SMFN(FN,H21,W,G, WRDD, DLl, XIL, DLll, WD,DL1D,H41 ,TH, . . . 

THD, DEFM, DEFMD) 



COEFFICIENTS OF SMALL MOTION ACCELERATIONS IN SMALL MOTION DYNAMICS 
EQUATIONS SUBROUTINE 

CALL SMMNN(XMNN,XMQQP,XML,A,XMU) 

ACCELERATION COEFFICIENTS MATRIX AND RIGHT-HAND SIDE VECTOR 
FORMULATION SUBROUTINE 

CALL BIGFOR(BIGM,BIGF,XMQQ,XMQN,FQ,XMNQ,XMNN,XKN,FN,U, DEFMD) 
LINEAR EQUATION SOLVER FOR ACCELERATIONS SUBROUTINE 
CALL XLEQ(BIGM,BIGF,SOL) 
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* 

* 

* 



INTEGRATE ACCELERATIONS AND THEN VELOCITY TO GET LARGE MOTION 
ANGULAR POSITION AND SMALL MOTION, LOCAL COORDINATE , TIP POSITION 



DO 5 1=1,2 
SOLl(I)=SOL(I) 

5 CONTINUE 
SORT 

VEL=INTGRL (VO , SOLI , 2 ) 

NOSORT 

THD(1)=VEL(1) 

DEFMD(1)=VEL(2) 

DO 10 1=1,2 
VEL1(I)=VEL(I) 

10 CONTINUE 
SORT 

POS=INTGRL ( POSO , VEL 1 , 2 ) 

NOSORT 

TH(l)=POS(l) 

ANG=TH(1) 

CUR=XIINP(1) 

DEFM(l)=POS(2) 

AC1=S0L1(1) 

AC2=S0L1(2) 

VE1=VEL(1) 

VE2=VEL(2) 

P01=P0S(1) 

P02=P0S(2) 

* 



ANGULAR ACCELERATION, VELOCITY, ANGLE. 



TAG2=ACl+AC2/0 . 9985D0 
TAGl=VEl+VE2/0 . 9985D0 
TAG =P01 +PO2/0.9985D0 



* COEFFICIENT OF THE CONTROL ALGORITHM 

CALL CCO(XMQQ,XMQN,XMNN,XMNQ,FTT,FN,XKN, . . . 

^ DEFM,CTB,FCO, DEFMD) 

* CALCULATE THE REQUIRED TORQUE 

^ CALL REQTO(RETO,CTB,TAGl,TAG,FCO,GKV,GKP,DEA) 

* INVERSE HYDRAUIC DYNAMIC 

'k 

* CALCULATE THE REQUIRED CURRENT 

CALL REC(RETO,PL,TE,DM,VT,THD,PS,CTM,XKV,REIC,BE) 
TORK=DETO(l) 

CU=DEICU) 

TERMINAL 
METHOD ADAMS 

PRINT 12.0E-3, TAG ,TORK ,CU 
SAVE 6.0E-3,TAG 

CONTROL FINTIM=5.000 ,DELT=0.006 
END 

PARAM GKV=4.0 ,GKP=4.0 
END 

PARAM GKV=8.0,GKP=16.0 

GRAPH (G1,DE=TEK618) TIME (UN=SEC) ,T0RK 

LABEL (Gl) L0AD=2.1151 KG 

GRAPH (G2,DE=TEK618) TIME (UN=SEC) , TAG 

LABEL (G2) LOAD=2.1151 KG 

GRAPH (G3,DE=TEK618)TIME,CU 

LABEL (G3) LOAD=2.1151 KG 

GRAPH (G4,DE=TEK618) TIME,TAG1 

LABEL (G4) LOAD=2.1l5l KG 

GRAPH (G5,DE=TEK618) TIME,TAG2 

LABEL (G5) LOAD=2.1151 KG 
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X* X- X- 



END 

STOP 



LISTING OF SUBROUTINES 



SUBROUTINE KINIT(TH,THD,DEFM,DEFMD ,VO,POSO,A,ML,MU,LL, 
#NR,E,ZI,PS,CTM,VT,BE,DM,KV,TE,QL,PL,PLIC ,REIC ) 

REAL*3 VO(2) ,POSO(2) ,ML,MU,LL,MR,TH,THD,DEFM,DEFMD, 
#A,E,ZI,ITORQ,PS, CTM,VT,BE,DM,KV,TE,QL,PL,PLIC, 
ttCl,C2,APl,AP2,BETAl ,BETA2,rEIC , 
#DEPL,DEQ,DI,DEFP,DEIC1,DEIC2 

ITORQ= 44.554950510000000 
DM=6.2271D-05 
TE=. 90000000000000 
PL=ITORQ/(DM*TE) 

PLIC=PL 

CTM=3.7064772D-13 

QL=CTM*PL 

KV=2.402963D-09 

PS=1.37888D+07 

VT=3.05127D-04 

BE=690.D6 

A=6.17795D-04 

ML=2. 1151000000000 

MU=7861. 05000000000000 

LL=0. 99850000000000 

MR=9. 00011451000000 

E=2.0D11 

ZI=4.065D-10 

VO (1 =0.000000000000000 

V0(2 )=0 . 000000000000000 

P0s0(l)=0.00000000000000 

P0S0(2)=- . 14606414900000 

TH=P0S0(1) 

THD=V0U) 

DEFM=P0S0(2) 

DEFMD=V0(2) 

REIC=4.0 

RETURN 

END 



DOUBLE PRECISION FUNCTION ONE(X) 

REALMS C1,C2, A1P,A2P,BETA1,BETA2 

COMMON/ FCDATA/Cl , C2 , AlP , A2P , BETAl , BETA2 



# 

# 

# 



0NE= 



RETURN 

END 



Cl* (AlP* (COS (BETA1*X)+C0SH 
(BETA1*X) ) + (SIN(BETAl*X)+SINH(BETAl’^X) ) ) + 
C2*(A2P*(COS(BETA2*X +COSH(BETA2*X))+ 
(SIN(BETA2*X)+SINH(BETA2*X) ) ) 



DOUBLE PRECISION FUNCTION TWO(X) 

REAL*8 C1,C2, AlP, A2P , BETAl ,BETA2 

COMMON/FCDATA/Cl , C2 , AlP , A2P , BETAl , BETA2 

TWO= (C1*(A1P*(C0S(BETA1*X)+C0SH 

# (BETA1*X) ) + (SIN(BETAl*X)+SINH(BETAl’^X) ) ) + 

# C2* ( A2P* ( COS ( BETA2*X) +COSH { BETA2*X ) ) + 

# (SIN(BETA2*X)+SINH(BETA2*X) ) ) ) **2 
RETURN 

END 

DOUBLE PRECISION FUNCTION SIX(X) 

REAL*8 C1,C2, AlP ,A2P , BETAl , BETA2 

COMMON/FCDATA/Cl , C2 , AlP , A2P , BETAl , BETA2 
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4 ^: 4 *: 4 *: 



SIX= .9985*CC1*(A1P*(C0S(BETA1*X)+C0SH 

(BETA1*X) ) + (SIN(BETAl*X)+SINH(BETAl’^X))) + 
C2*(A2P*(C0S(BETA2*X)+C0SH(BETA2*X))+ 

(SIN(BETA2*X)+SINH(BETA2*X) ) ) )+ 
X*(C1*(A1P*(C0S(BETA1*X)+C0SH 
(BETA1*X))+(SIN(BETA1*X)+SINH(BETA1^X)))+ 
C2* ( A2P* ( COS ( BETA2*X) +COSH( BETA2*X) ) + 
(SIN(BETA2*X)+SINH(BETA2*X))) ) 

RETURN 

END 



DOUBLE PRECISION FUNCTION EIGHT (X) 

REAL*8 C1,C2, AlP ,A2P ,BETA1 ,BETA2 

COMMON/FCDATA/Cl , C2 , AlP , A2P , BETAl , BETA2 

EIGHT= (C1*BETA1*BETA1*(A1P*(-C0S 

(BETAl*X)+COSH(BETAl*X))+(-SIN(BETAl*X)+SINH(BETAl*X)))+ 
C2*BETA2*BETA2* ( A2P* ( -COS ( BETA2*X) +COSH ( BETA2*X) ) + 
(-SIN(BETA2*X)+SINH(BETA2*X) ) ) )^*2 

RETURN 

END 



SUBROUTINE FORM ( W , WTH , WD , DL 1 , DLID , XIL , XIR , ARTH , WRDD , ARRDD , U , UD , 
#XMQQP,G,H11,H21,DL11 ,H41 ,XK11 ,A,MU,ML ,LL,TH,THD ,DEFM,DEFMD , 

# E.ZI,MR,MX \ 

REAL*8 W(3,3) ,WTH(3,3) ,WD(3,3) ,DL1(3,3) ,DL1D(3,3) ,XIL(3,3) 
REAL*8 XIR(3,3),ARTH(3,3),WRDD(3,3),ARRDD(3,3),U ,UD 
REAL*8 XMQQP , G(3 , 1 ) /Hll (1 , 3KH21 (1 , 3) ,DL11 (3 , 3) 



REAL*8 H41(l,3) ,XK11 
REAL* 8 THD,DEFM,DEFMD 
REAL*8 Cl, AlP, BETAl 
REAL*8 ONE, TWO, EIGHT, Y 
EXTERNAL ONE , TWO , EIGHT 
1 , 1 )= 1. 00000000000000 



,MU,ML,LL,MR,MX,TH 

,A,E,ZI 



1 , 2 )= 0. 00000000000000 

1,3)=0. 00000000000000 

2.1) =LL*DCOS(TH) 

2.2) =DCOS(TH) 

2.3) =-DSIN(TH) 

3.1) =LL*DSIN(TH) 

3.2) =DSIN(TH) 



W 
W 
W 
W 
W 
W 
W 
W 

W^3;3)=DCOS(TH) 

wth(- - ' - 

WTH I 
WTH( 

WTH( 

WTH( 

WTH( 

WTH< 

WTH( 



1,1)=0. 00000000000000 
1 , 2 )= 0 .00000000000000 
1 , 3 )= 0. 00000000000000 

2.1) =-LL*DSIN(TH) 

2.2) =-DSIN(TH) 

2.3) =-DCOS(TH) 

3.1) =LL*DC0S(TH) 

3.2) =DCOS(TH) 



WTH<3,3)=-DSIN(TH) 



1 , 1 )= 0. 00000000000000 
1 , 2 )= 0. 00000000000000 
1 , 3 )= 0. 00000000000000 

2.1) =-LL*DSIN(TH)*THD 

2.2) =-DSIN(TH)*THD 

2.3) =-DCOS(TH)*THD 

3.1) =LL*DCOS(TH)*THD 

3.2) =DCOS(TH)*THD 



DL1(1,1)=1. 00000000000000 
DL1(1,2)=0. 00000000000000 
DL1(1, 3 )=0. 00000000000000 
DLl ( 2 , 1 )=0 . 00000000000000 
DL1( 2, 2)=1. 00000000000000 
DLl (2, 3 )=0. 0000000000000 
DL1(3,1)=DEFM 



76 



DLl ( 3 , 2 )=0 . 000000000000000 
DLl (3, 3 )=1. 00000000000000 
DLID ( 1 , 1 )=0 . 00000000000000 
DL1D( 1 , 2)=0 . 00000000000000 
DL1D(1,3)=0. 00000000000000 
DL1D(2,1)=0. 00000000000000 
DL1D(2,2)=0. 0000000000000 
DL1D(2 , 3 )=0 . 000000000000 
DL1D{3,1)=DEFMD 
DL1D( 3 , 2 )=0 . 000000000000 
DLID ( 3 , 3 )=0 . 00000000000000 
XIL(1,1)=ML 

XIL(1, 2 )=0. 01673545900000 
XIL( 1,3)=. 00000000000000 
XIL ( 2, l) =0.01678545900000 
XIL 2,2)=1.77679D-04 
XIL(2,3)=0. 00000000000000 
XIL ( 3, l)=. 00000000000000 
XIL ( 3 , 2 )=0 . COOOOOOOOOOOOO 
XIL(3,3)=2.986791D-03 
MX=. 01678545900000 
XIR ( 1 1 ) =MR 

XIR ( 1 ) 2 )=0 . 000000000000000 
XIR(1, 3 )=0. 000000000000000 
XIR(2 , 1 )=0 . 000000000000000 
XIR(2,2)=. 027467 130000000 
XIR(2,3)=0. 000000000000000 
XIR(3,l)=0. 000000000000000 
XIR (3, 2 )=0. 000000000000000 
XIR( 3, 3) =.02746713000000 
ARTH(1,1)=0. 00000000000000 
ARTH( 1, 2 )=0. 00000000000000 
ARTH(1, 3 )=0. 00000000000000 
ARTH( 2, l)=0. 00000000000000 
ARTH(2,2)=-DSIN(TH) 
ARTH(2,3)=-DC0S(TH) 

ARTH(3 , 1 )=0 . 00000000000000 

ARTH(3,2)=DCOS(TH) 

ARTH(3,3)=-DSIN(TH) 

WRDD(l,l)=0. 00000000000000 
WRDD(1, 2 )=0. 00000000000000 
WRDD(1,3)=0. 00000000000000 
WRDD(2,l)=-LL*DCOS(TH)*{THD**2) 
WRDD(2,2)=-DCOS(TH)*(THD**2) 
WRDD(2,3)=DSIN(TH)*(THD**2) 
WRDD(3,1)=-LL*DSIN(TH)*(THD**2) 
WRDD 3,2l=-DSIN(TH)*(THD**2) 
WRDD(3,3)=-DC0S(TH)*(THD**2) 
ARRDD( 1 , 1 )=0 . 00000000000000 
ARRDD ( 1 , 2 )=0 . 00000000000000 
ARRDD ( 1 , 3 )=0 . 00000000000000 
ARRDD ( 2 , 1 )=0 . 00000000000000 
ARRDD(2,2)=-DCOS(TH)*(THD**2) 
ARRDD(2,3)=DSIN(TH)^(THD**2) 
ARRDD( 3 , 1 )=0 . 00000000000000 
ARRDD(3,2)=-DSIN(TH)*(THD**2) 
ARRDD (3,3)=-DC0S(TH)*( THD** 2 ) 

U =DEFM 

UD =DEFMD 

CALL DQG4(-LL,O.DO,TWO,Y) 

XMQQP =Y 
G(1,1)=0. 00000000000000 
G( 2, l)=0. 00000000000000 
G(3,1)=-9. 80660000000000 
Hll( 1,1) =4. 85651900000000 
Hll(l,2)=-2. 42825869300000 
Hll ( 1 , 3 )=0 . 00000000000000 
H2l(l,l)=0, 00000000000000 
H21 ( 1 , 2 )=0 . 00000000000000 
CALL DQG4(-LL,-O.DO,ONE ,Y) 
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>h yh 



H21(l,3)=A’^MU*Y 
DO 50 1=1,3 
DO 60 J=l,3 

DL11U,J)=0. 00000000000000 
60 CONTINUE 
50 CONTINUE 

DLll (3,1 >=1.00000000000000 
H41(1,1)=ML 

H41 ( 1 , 2 )=0 . 000000000000000 

H41( 1,3 >=.016785459000000000 

C.ALL DQG4(-LL,0. DO, EIGHT, y) 

XKll =Y*E*ZI 

RETURN 

END 

* 

■k 



SUBROUTINE XLMMQQ ( MQQ , U , XMQQP , DL 1 , WTH , ARTH , XIL , XIR , A , MU , TH , DEEM > 



)Q,UT,P,DLlf (3,3> ,WtHT(3,3) ,ARTHT(3,3) ,P1(3,3) 
H3,3hP3(3,3>,P4(3,3>,P5(^,3>,P6(^3j,P7b,3K 



REAL*8 MC 

REALMS • V-/. 

REAL*8 U ,XMQQP,DL1U/3> ,WTH(3,3KARTH(3,3> ,XIL(3,3) 
REAL*8 XIR(3,3> ,A,TH,DEFM ,SP,TP 
M=2 



MU 



L=1 

N=3 

MQQ=0 . 00000000000000 
CALL TRANS(U,UT,L,L> 

C.ALL MATMUL(UT, XMQQP, L,L,L,P> 

CALL MATMUL(P,U,L,L,L,SP) 

CALL TRANS(DL1,DL1T,N,N> 

CALL TRANS ( ARTH, ARTHT,N,N> 

CALL TRANS ( WTH, WTHT,N,N> 

CALL MATMUL(WTH,DL1,N,N,N,P1> 

CALL MATMUL(P1,XIL,N,N,N,P2) 

CALL MATMUL(P2,DL1T,N,N,N,P3) 

CALL MATMUL(P3,WTHT,N,N,N,P4> 

CALL MATMUL(ARTH,XIR,N,N,N,P5) 

CALL MATMUL(P5,ARTHT,N,N,N,P6> 

CALL MATADD(P4,P6,N,N,P7> 

CALL TRACE (P7,N, TP > 
MQQ=((1./3.>*A*MU> + (A*MU*SP> + TP 
RETURN 
END 



SUBROUTINE XLMMQN ( XMQN , A , MU , ML , LL , MX ,DEFM > 

REAL*8 XMQN ,MU,ML,LL,MX,A, DEFM 

REAL*8 Cl,AlP,BETAl,y,SIX , C2 , A2P ,BETA2 
EXTERNAL SIX 

CALL DQG4(-LL,0.D0,SIX ,Y) 

XMQN =(A*MU*Y>+(ML*LL)+MX 

RETURN 

END 



SUBROUTINE XLMFQ ( FQ , U , XMQQP , DLl , WTH , ARTH ,XIL,XIR,UD,H11,G,H21, 
#WRDD , DL 1 D , WD , ARRDD , H4 1 , TH , THD , DE FM , DE FMD , A , MU , ML , LL , 

#TORQUE,FTT> 

REAL*8 FQP,P ,P1 (1, 3 > ,P2 (1 , 3> ,P3 ( 1 , 3> ,P4 (3 ,3> ,P5 (3 , 3 > 

REAL*8 P5(3 3KP7(i,3j,P8(i,3j,P9(3,3j,P10(3.i>,Pll(1.3>,P12(l,3> 
REAL*8 FPFH(3,3>,FHP(3,3>,FPT(3,3> ,DL1DT{3,3) , WDT(3 , 3 ) , ARRDDT(3 , 3> 
REAL*8 UT,DL1T(3,3> , WRDDT(3 , 3> , FPF(3 , 3> ,FPS (3 , 3 > ,WTHT(3 , 3> 

REAL*8 U, XMQQP ,DL1(3 , 3> , WTH(3 ,3 > , ARTH( 3 , 3) ,XILU - 3) 

REAL*8 XIR(3,3>,UD,H11U,3> ,G(3,1>,H21(1,3) .WRDD(3,3> 

REALMS DL1D(3,3KWD(3,3> ,ARRDD(3,3) ,H41 (1 , 3) ,MU,LL ,ML 
REALMS A , TORQUE , FQ , TH , THD , DEFM , DEFMD 
REAL*8 FP,SP,TP,TFP,FTHP ,FTT 
M=2 
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L=1 

N=3 

CALL TRANS(U,UT,L,L) 

FQP=XMQQP ’^THD 

CALL MATMUL(UT,FQP,L,L,L,P) 

CALL MATMUL(P,UD,L,L,L,FP) 

CALL TRANS (WTH,WTHT,N,M) 

CALL MATMUL(H11,WTHT,L,N,N,P1) 

CALL MATMUL(P1,G,L,N,L,SP) 

CALL MATMUL(UT,H21,L,M,N,P2) 

CALL MATNUL(P2,WTHT,L,N,N.P3) 

CALL MATMUL(P3,G,L,N,L,TP) 

CALL TRANS(DL1,DL1T,N,N) 

CALL TRAMS (WRDD,WRDDT,N,N) 

CALL MATMUL(WTH,DL1,N,N,N,P4) 

CALL MATMUL(P4,XIL,N,N,N,P5) 

CALL MATMUL(P5,DL1T,N,N,N,P6) 

CALL MATMUL { P6 , WRDDT , N , N , N , FPF ) 

CALL TRANS(DL1D,DL1DT,N,N) 

CALL TRANS (WD,WDT,N,N) 

CALL MATMUL (WTH,DL1,N,N,N,P7) 

CALL MATMUL(?7,XIL,N,N,N,P8) 

CALL MATMUL (P8,DL1DT,N,N,N,P9) 

CALL MATMUL (P9,WDT,N,N,N,FPS) 

CALL TRANS (ARRDD,ARRDDT,N,N) 

CALL MATMUL(ARTH,XIR,N,N,N,P10) 

CALL MATMUL(P10,ARRDDT,N,N,N,FPT) 

DO 30 1=1,3 
DO 40 J=l,3 
FPS(I, J)=FPS(I, J)*2. 

40 CONTINUE 

30 CONTINUE 

CALL MATADD(FPF,FPS,N,N,FPFH) 

CALL MATADD(FPFH,FPT,N,N,FHP) 

CALL TRACE (FHP,N,TFP) 

CALL MATMUL (H41,DL1T,L,N,N, PI 1) 

CALL MATMUL(P11,WTHT,L,N,N,P12) 

CALL MATMUL(P12,G,L,N,L,FTHP) 

FTT=(-2.*A*MU*FP) + SP + TP - TFP + FTHP 

FQ=FTT+TORQUE 

RETURN 

END 
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SUBROUTINE SMKN (XKN , XKl 1 , XMQQP , A , MU , THD ) 
REAL*8 XKN,KNP .XMQQP ,XK11 , A, THD, MU 
KNP =XMQQP *(-A)*MU*(THD**2) 

XKN =KNP +XK11 

RETURN 

END 



SUBROUTINE SMMNQ (XMNQ , DLl , WTH , XIL , DLl 1 , W , TH , DEFM , A , MU , 

REAL*8 XMNQ ,DL11T(3 
REAL*8 P3(3,3) ,P4(3, 

REAL*8 W(3,3),DL11(3 
REAL*8 C1,A1P,BETA1 
REAL*8 SIX,Y ,C2,A2P,BETA2 
EXTERNAL SIX 
M=2 
L=1 
N=3 

CALL TRANS(DL11,DL11T,N,N) 

CALL TRANS(W,WT,N,N) 

CALL MATMUL(WTH,DL1,N,N,N,P1) 

CALL MATMUL(P1,XIL,N,N,N,P2) 

CALL MATMUL(P2,DL11T,N,N,N,P3) 



,3),WT(3,3),P1(3,3) P2(3 3) 

3) .DLl (3,3) , WTH (3, 3) ,XIL(3,3) 



,3) ,TH,DEFM ,A,MU,LL 
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CALL MATMUL(P3,WT,N,N,N,P4) 
CALL TRACE (P4,N,TFP1) 

CALL DQG4(-LL,0.D0,SIX ,Y) 
XMNQ =TFP1 + (Y*A*MU) 

RETURN 

END 



SUBROUTINE SMFN(FN,H21 , W ,G,WRDD,DL1 ,XIL , DLll , WD ,DL1D ,H41 ,TH, 

#THD,DEFM,DEFMD) 

REAL*8 FN Pl(l,3) P2(3,3),P3(3,3),P4(3,3),P5(3,3),P6(3,3) 

REALM'S P7(3,3),P8(3,3),P9(3,3) 

REAL*8 P14(l,3) ,P15U,3) ,TP,FP ,SP 

REAL*8 FN1(3,3), G(3 , 1 ) ,H21 ( 1 , 3 ) , WRDD ( 3 , 3 ) , DLID (3 , 3) 

REAL*8 WD(3,3),H41(1,3) ,XIL(3,3) ,W(3,3) ,DL11(3,3) 

REAL*8 DL1(3,3) ,DL11T(3,3) ,WT{3,3) 

REAL*8 TH , THD , DEFM , DEFMD , TFNl , FN3 

N=2 

L=1 

N=3 

CALL TRANS (W,WT,N,N) 

CALL MATMUL(H21,WT,L,N,N,P1) 

CALL MATMUL(P1,G,L,N,L,FP) 

CALL TRANS(DL11,DL11T,N,N) 

CALL MATMUL(WRDD,DL1,N,N,N,P2) 

CALL MATMUL(P2,XIL,N,N,N,P3) 

CALL MATMUL(P3,DL11T,N,N,N,P4) 

CALL MATMUL(P4,WT,N,N,N,P5) 

CALL MATMUL(WD,DL1D,N,N,N,P6) 

CALL MATMUL(P6,XIL,N,N,N,P7) 

CALL MATMUL(P7,DL11T,N,N,N,P8) 

CALL MATMUL(P8,WT,N,N,N,P9) 

DO 10 1=1,3 
DO 20 J=l,3 
P9(I,J)= P9(I,J)*2. 

20 CONTINUE 
10 CONTINUE 

CALL MATADD(P5,P9,N,N,FN1) 

CALL TRACE (FN1,N, TFNl) 

SP =TFN1 

CALL HATMUL(H41,DL11T,L,N,N,P14) 

CALL MATMUL(P14,WT,L,N,N,P15) 

CALL MATMUL(P15,G,L,N,L,FN3) 

TP =FN3 

FN=FP -SP + TP 

RETURN 

END 



SUBROUTINE SMMNN ( XMNN , XMQQP , ML , A , MU ) 

REAL*8 XMNN, XMQQP , ML, MU, A 
XMNN =ML 

XMNN =XMNN + XMQQP *A*MU 

RETURN 

END 



SUBROUTINE MATMUL (A - B ,M , L ,N , C) 

REAL*8 A(M,L) ,B(L,N) ,C(M,N) 

DO 10 1=1, M 
DO 20 J=1,N 
C(I, J)=0.0 
DO 30 INDEX=1,L 

C(I,J)=C(I,J) + A(I,INDEX)*B(INDEX,J) 
30 CONTINUE 
20 CONTINUE 
10 CONTINUE 
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RETURN 

END 



SUBROUTINE TRANS(A,B,M,L) 
REALMS A(M,L) ,B(L,M) 

DO 10 1=1, M 
DO 20 J=1,L 
B(J,I)=A(I,J) 

20 CONTINUE 
10 CONTINUE 
RETURN 
END 



SUBROUTINE TRACE ( A, M, TRAC) 
REALMS A(M,M) 

TRAC=0 . 0 
DO 10 1=1, M 
TRAC=TRAC + A(l,I) 

10 CONTINUE 
RETURN 
END 

MATRIX ADDITION SUBROUTINE 

SUBROUTINE MATADD(A,B,M,L,C) 
REALMS A(M,L) ,B(M,L) ,C(M,L) 
DO 10 1=1, M 
DO 20 J=1,L 
C(I,J)=A(I,J) + B(I,J) 

20 CONTINUE 
10 CONTINUE 
RETURN 
END 



SUBROUTINE BIGFOR ( BIGM , BIGF , MQQ , XMQN , FQ , XMNQ , XMNN , XKN , FN , U , DEFMD ) 

REALMS BIGM(2,2),BIGF(2,1),XMQN ,XMNQ ,XMNN ,XKN 

REALMS FN ,U ,MQQ,P ,FQ, DEFMD 

M=2 

L=1 

BIGM(1,1)=MQQ 
BIGM(1,2)=}SqN 
BIGM(2,1)=XMNQ 
BIGM(2,2)=XMNN 
BIGF(1 ,1)=FQ 

CALL MATMUL(XKN,U,L,L,L,P) 

3IGF(2,1)=FN -P 

RETURN 

END 



SUBROUTINE XLEQ(BIGM, BIGF, SOL) 

REALMS BIGM(2,2),BIGF(2,1) ,SOL(2),WKAREA(18) 

M=1 

N=2 

CALL LEQT2F (BIGM, M,N,N, BIGF, M, WKAREA, lER) 

DO 10 1=1,2 
SOL(I)=BIGF(I,l) 

10 CONTINUE 
RETURN 
END 



SUBROUTINE LEQT2F (IMSL) 
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PURPOSE 



LINEAR EQUATION SOLUTION - FULL STORAGE 
MODE - HIGH ACCURACY SOLUTION 



USAGE 



CALL LEQT2F (A,M,N , lA, B , IDGT , WKAREA, lER) 



ARGUMENTS A 



M 

N 

lA 



B 



IDGT 



WKAREA 

lER 



REQD. IMSL ROUTINES 



INPUT MATRIX OF DIMENSION N BY N CONTAINING 
THE COEFFICIENT MATRIX OF THE EQUATION 
AX = B. 

NUMBER OF RIGHT-HAND SIDES. (INPUT) 

ORDER OF A AND NUMBER OF ROWS IN B. (INPUT) 

ROW DIMENSION OF A AND B EXACTLY AS SPECIFIED 
IN THE DIMENSION STATEMENT IN THE CALLING 
PROGRAM. (INPUT) 

INPUT MATRIX OF DIMENSION N BY M CONTAINING 
THE RIGHT-HAND SIDES OF THE EQUATION AX = B . 
ON OUTPUT, THE N BY M MATRIX OF SOLUTIONS 
REPLACES B. 

INPUT OPTION. 

IF IDGT IS GREATER THAN 0, THE ELEMENTS OF 
A AND B ARE ASSUMED TO BE CORRECT TO IDGT 
DECIMAL DIGITS AND THE ROUTINE PERFORMS 
AN ACCURACY TEST. 

IF IDGT EQUALS 0, THE ACCURACY TEST IS 
BYPASSED . 

ON OUTPUT,' IDGT CONTAINS THE APPROXIMATE 
NUMBER OF DIGITS IN THE ANSWER WHICH 
WERE UNCHANGED AFTER IMPROVEMENT. 

WORK AREA OF DIMENSION GREATER THAN OR EQUAL 
TO N**2+3N. 

ERROR PARAMETER. (OUTPUT) 

WARNING ERROR 

lER = 34 INDICATES THAT THE ACCURACY TEST 
FAILED. THE COMPUTED SOLUTION MAY BE IN 
ERROR BY MORE THAN CAN BE ACCOUNTED FOR 
BY THE UNCERTAINTY OF THE DATA. THIS 
WARNING CAN BE PRODUCED ONLY IF IDGT IS 
GREATER THAN 0 ON INPUT. (SEE THE 
CHAPTER L PRELUDE FOR FURTHER DISCUSSION.) 

TERMINAL ERROR 

lER = 129 INDICATES THAT THE MATRIX IS 
ALGORITHMICALLY SINGULAR. (SEE THE 
CHAPTER L PRELUDE). 

lER = 131 INDICATES THAT THE MATRIX IS TOO 
ILL-CONDITIONED FOR ITERATIVE IMPROVEMENT 
TO BE EFFECTIVE. 

S INGLE / LUDATN , LUELMN , LUREFN , UERTST , UGET 10 

DOUBLE/LUDATN , LUELMN , LUREFN , UERTST , UGETIO , 
VXADD,VXMUL,VXSTO 



SUBROUTINE LEQT2F (A ,M,N, IA,B, IDGT, WKAREA, lER) 
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DIMENSION A(IA,1),B(IA,1) ,WKAREA(1) 

DOUBLE PRECISION A, B, WKAREA, D1,D2,WA 

FIRST EXECUTABLE STATEMENT 
INITIALIZE lER 

IER=0 
JER=0 
J = N*N+1 
K = J+N 
MM = K+N 
KK = 0 
MMl = MM-1 
JJ=1 

DO 5 L=1,N 

DO 5 1=1, N 

WKAREA(JJ)=A(I,L) 

JJ=JJ+1 

CONTINUE 

DECOMPOSE A 
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CALL LUDATN (WKAREA ,N ,N , A , lA , IDGT , D1 , D2 , WKAREA( J) , WKAREA(K) , 
WA,IER) 

IF (IER.GT.128) GO TO 25 

IF (IDGT .EQ. 0 .OR. lER .NE. 0) KK = 1 

DO 15 I = 1,M 

PERFORMS THE ELIMINATION PART OF 
AX — B 

CALL LUELMN (A, IA,N,B(1 , I ) ,WKAREA( J) ,WKAREA(MM) ) 

' REFINEMENT OF SOLUTION TO AX = B 

IF (KK .NE. 0) 

CALL LUREFN (WKAREA ,N,N , A , lA ,B( 1 , 1 ) , IDGT ,WKAREA( J) ,WKAREA(MM) , 
^ WKAREA(K) ,WKAREA(K) , JER) 

DO 10 11=1, N 

B(II,I) = WKAREA(MM1+II) 

10 CONTINUE 

IF (JER.NE.O) GO TO 20 
15 CONTINUE 
GO TO 25 
20 lER = 131 
25 JJ=1 

DO 30 J = 1,N 

DO 30 I = 1,N 

A(I, J)=WKAREA(JJ) 

JJ=JJ+1 

30 CONTINUE 

IF (lER .EQ. 0) GO TO 9005 
9000 CONTINUE 

CALL UERTST ( lER , 6HLEQT2F ) 

9005 RETURN 
END 



SUBROUTINE DQG4 (IMSL SUBROUTINE) 

PURPOSE 

TO COMPUTE INTEGRAL(FCT(X) , SUMMED OVER X FROM XL TO XU) 
USAGE 

CALL DQG4 (XL , XU , FCT , Y) 

PARAMETER FCT REQUIRES AN EXTERNAL STATEMENT 
DESCRIPTION OF PAElAflETERS 

XL - DOUBLE PRECISION LOWER BOUND OF THE INTERVAL. 

XU - DOUBLE PRECISION UPPER BOUND OF THE INTERVAL. 

FCT - THE NAME OF AN EXTERNAL DOUBLE PRECISION FUNCTION 
SUBPROGRAM USED. 

Y - THE RESULTING DOUBLE PRECISION INTEGRAL VALUE. 

METHOD 

EVALUATION IS DONE BY MEANS OF 4-POINT GAUSS QUADRATURE 
FORMULA, WHICH INTEGRATES POLYNOMIALS UP TO DEGREE 7 
EXACTLY. FOR REFERENCE, SEE 

V. I. KRYLOV, APPROXIMATE CALCULATION OF INTEGRALS, 
MACMILLAN, NEW YORK/LONDON, 1962, PP. 100-111 AND 337-340. 

SUBROUTINE DQG4(XL,XU,FCT ,Y) 

DOUBLE PRECISION XL,XU,Y,A,B,C,FCT 
A=.5D0’^(XU+XL) 

B=XU-XL 

C=.43056815579702629D0*B 

Y=.17392742256872693D0*(FCT(A+C)+FCT(A-C) ) 

C=. 1699905217924281300*8 

Y=B*(Y+.32607257743127307D0*(FCT(A+C)+FCT(A-C))) 

RETURN 

END 



SUBROUTINE CCO (XMQQ , XMQN , XMNN , XMNQ , FTT , FN , XKN , DEFM , CTB , FCO , 
#DEFMD) 

REAL*8 XMQQ, XMNN, XMQN, FTT, FN, XKN, U, CTB, FCO, CTA,B11,B22 
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REAL*8 B33,B44,F11,F22,DEFMD,F33 
Bll=XMQQ/0.9985 
B22=XMQN-B11 
B33=XMNQ/0.9985 
B44=XMNN-B33 
CTA=B22/B44 
CTB=XMQQ-CTA*XMNQ 
F11=CTA*FN 
F22=(CTA*XKN)*DEFM 
F33= ( CTA*0 . 00 ) =^DEFMD 
FC0=F11-F22-FTT -F33 
RETURN 
END 



SUBROUTINE REQTO ( RETO , CTB , TAGl , TAG , FCO , GKV , GKP , DEA) 
REAL*8 DETOl , DET02 , rETO , GKV , GKP , DEA , TAGl , TAG , FCO , CTB 
DET01=(-GKV*TAG1)+GKP*(DEA-TAG) 

DET02=CTB*DET01 

RET0=DET02+FC0 

RETURN 

END 



SUBROUTINE REC ( RETO , PL , TE , DM , VT , THD , PS , CTM , XKV , DEIC , BE ) 
REAL*8 RETO, PL, TE,DM,VT,THD,PS ,CTM,XKV,REIC ,BE 
REALMS DI,DEQ,DEFP,DEIC1,DEIC2 
DEPL =DETO /(TE *DM ) 

DI =VT *(DEPL -PL )/(4.0D0*BE *0.006D0) 

DEO =DM ’^THD +CTM *DEPL +DI 
DEFP = (PS - DEPL ) 

IF(DEFP .LT.0.0) GO TO 13 
DEIC1=DSQRT(DEFP) 

GO TO 14 

13 DEFP=-DEFP 

DEIC1=DSQRT(DEFP) 

14 DEIC2=DEIC1*XKV 

REIC =DEQ /(DEIC2 ) 

RETURN 

END 
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